#include "AlgoNSGA.h"
#include "AlgoEvaluator.h"
#include "AlgoRRTStar.h"
#include "DebugConfig.h"
#include "ResultSaver.h"
#include "AlgoEvaluatorDB.h"
#include <memory>
#include <iostream>



int main() 
{
    // 0. Load Debug Configuration
    DebugConfig::getInstance().load("debug_config.ini");

	std::shared_ptr<algo::AlgoEvaluator> pAlgo;
	pAlgo.reset(new algo::AlgoEvaluator); // Replace with actual AlgoEvaluator instance

	algobase::TaskConfig config;
	config.mStartTime = algobase::Date(2023, 1, 0);
	config.mEndTime = algobase::Date(2023, 1, 0);
	config.mMinLongitude = 0;
	config.mMaxLongitude = 20;
	config.mMinLatitude = 0;
	config.mMaxLatitude = 20;
	config.mDateType = algobase::DT_Month;
	config.mStartPos = {13, 13};
	config.mEndPos = {14, 14}; // Adjusted for a more meaningful path

	pAlgo->setTaskConfig(config);

	algobase::EvaluateL2IndexWeightVector Safe, Stealth, Economy, Completion;
	Safe.emplace_back(algobase::EvaluateL2IndexType::EL2IT_1EnvironmentalNoise, 0.1);
	Safe.emplace_back(algobase::EvaluateL2IndexType::EL2IT_3CPCD, 0.4);
	Safe.emplace_back(algobase::EvaluateL2IndexType::EL2IT_4AcousticStealth, 0.5);
	Stealth.emplace_back(algobase::EvaluateL2IndexType::EL2IT_6NavigationDistance, 0.2);
	Stealth.emplace_back(algobase::EvaluateL2IndexType::EL2IT_7NavigationTime, 0.3);
	Stealth.emplace_back(algobase::EvaluateL2IndexType::EL2IT_8NavigationEnergy, 0.2);
	Stealth.emplace_back(algobase::EvaluateL2IndexType::EL2IT_13Motivation, 0.5);
	Economy.emplace_back(algobase::EvaluateL2IndexType::EL2IT_9EndPointCompletion, 0.1);
	Economy.emplace_back(algobase::EvaluateL2IndexType::EL2IT_10TimeCompletion, 0.1);
	Economy.emplace_back(algobase::EvaluateL2IndexType::EL2IT_11MandatoryAreaCompletion, 0.8);
	Completion.emplace_back(algobase::EvaluateL2IndexType::EL2IT_12HardRestriction, 0.1);
    Completion.emplace_back(algobase::EvaluateL2IndexType::EL2IT_14DangerousAreaExposure, 0.9);

	algobase::EvaluateL1IndexMap L1Indexmap;
	L1Indexmap[algobase::EL1IT_Safety].first = 0.2;
	L1Indexmap[algobase::EL1IT_Safety].second = Safe;
	L1Indexmap[algobase::EL1IT_Stealth].first = 0.3;
	L1Indexmap[algobase::EL1IT_Stealth].second = Stealth;
	L1Indexmap[algobase::EL1IT_Economy].first = 0.3;
	L1Indexmap[algobase::EL1IT_Economy].second = Economy;
	L1Indexmap[algobase::EL1IT_Completion].first = 0.2;
	L1Indexmap[algobase::EL1IT_Completion].second = Completion;
	pAlgo->setEvaluateL1IndexMap(L1Indexmap);

	algobase::Paths rrt_star_paths;
	//std::vector<algobase::TrajectoryCoords> nsga_paths;
	std::vector <algobase::FinalPathScore> nsga_paths;

    DEBUG_LOG("TestRunner", "--- RRT* Phase ---");
	algo::AlgoRRTStar rrt_star;
    rrt_star.setDepthRange(-200.0, -50.0);
    auto db = std::make_shared<algo::AlgoEvaluatorDB>(config, std::vector<algobase::EvaluateL2IndexType>{algobase::EL2IT_3CPCD});


	//
	std::vector<double> lon_scale = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10};
	std::vector<double> lat_scale = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10};
	std::vector<float> arr_data(lon_scale.size() * lat_scale.size(), 1);
	float total = 0;
	for (size_t i = 0; i < lon_scale.size() * lat_scale.size(); ++i){
		total += i;
	}
	for (size_t i = 0; i < lon_scale.size() * lat_scale.size(); ++i) {
		arr_data[i] = i / total;
	}
	darr::DimsArray<float> area_2023_1_0_data({ DIM_LON_NAME, DIM_LAT_NAME }, { lon_scale, lat_scale }, arr_data);

	algobase::Date date(2023, 1, 0, 0);
	std::map<std::string, darr::DimsArray<float>> data_area;
	data_area[date.to_string()] = area_2023_1_0_data;
	db->appendData(algobase::EEDT_AreaSound, data_area);


	std::vector<float> wind_data(lon_scale.size() * lat_scale.size(), 1);
	/*float total = 0;
	for (size_t i = 0; i < lon_scale.size() * lat_scale.size(); ++i) {
		total += i;
	}
	for (size_t i = 0; i < lon_scale.size() * lat_scale.size(); ++i) {
		arr_data[i] = i / total;
	}*/
	darr::DimsArray<float> wind_2023_1_0_data({ DIM_LON_NAME, DIM_LAT_NAME }, { lon_scale, lat_scale }, wind_data);
	std::map<std::string, darr::DimsArray<float>> data_wind;
	data_wind[date.to_string()] = wind_2023_1_0_data;
	db->appendData(algobase::EEDT_Wind, data_wind);


	std::vector<float> Rain_data(lon_scale.size() * lat_scale.size(), 1);
	/*float total = 0;
	for (size_t i = 0; i < lon_scale.size() * lat_scale.size(); ++i) {
		total += i;
	}
	for (size_t i = 0; i < lon_scale.size() * lat_scale.size(); ++i) {
		arr_data[i] = i / total;
	}*/
	darr::DimsArray<float> Rain_2023_1_0_data({ DIM_LON_NAME, DIM_LAT_NAME }, { lon_scale, lat_scale }, Rain_data);
	std::map<std::string, darr::DimsArray<float>> data_Rain;
	data_Rain[date.to_string()] = Rain_2023_1_0_data;
	db->appendData(algobase::EEDT_Rain, data_Rain);

	
	std::vector<float> ShipNoise_data(lon_scale.size() * lat_scale.size(), 1);
	/*float total = 0;
	for (size_t i = 0; i < lon_scale.size() * lat_scale.size(); ++i) {
		total += i;
	}
	for (size_t i = 0; i < lon_scale.size() * lat_scale.size(); ++i) {
		arr_data[i] = i / total;
	}*/
	darr::DimsArray<float> ShipNoise_2023_1_0_data({ DIM_LON_NAME, DIM_LAT_NAME }, { lon_scale, lat_scale }, ShipNoise_data);
	std::map<std::string, darr::DimsArray<float>> data_ShipNoise;
	data_ShipNoise[date.to_string()] = ShipNoise_2023_1_0_data;
	db->appendData(algobase::EEDT_ShipNoise, data_ShipNoise);
	pAlgo->setEvaluateDB(db);
	//dummy data
	

	int exec_code = rrt_star.exec(config, db.get(), rrt_star_paths);
    DEBUG_LOG("TestRunner", "RRT* finished with code " << exec_code << ". Found " << rrt_star_paths.size() << " initial paths.");
	
    if (rrt_star_paths.empty()) {
        DEBUG_LOG("TestRunner", "RRT* generated 0 paths. The test cannot proceed to NSGA.");
        return 0;
    }

    // Ensure the population size is a multiple of 4 for NSGA-II
    size_t pop_size = rrt_star_paths.size();
    size_t remainder = pop_size % 4;
    if (remainder != 0) {
        size_t num_to_add = 4 - remainder;
        for(size_t i = 0; i < num_to_add; ++i) {
            rrt_star_paths.push_back(rrt_star_paths[i % pop_size]); // Duplicate from the start
        }
        DEBUG_LOG("TestRunner", "Adjusted population size from " << pop_size << " to " << rrt_star_paths.size() << " to be a multiple of 4.");
    }


	algo::AlgoNSGA nsga;

    DEBUG_LOG("TestRunner", "\n--- NSGA Phase ---");
    DEBUG_LOG("TestRunner", "Optimizing initial population of " << rrt_star_paths.size() << " paths...");
	
    int nsga_code = nsga.NSGA(pAlgo.get(),
		algobase::NSGAParams(),
		rrt_star_paths,
		nsga_paths);

    DEBUG_LOG("TestRunner", "NSGA finished with code " << nsga_code << ". Produced " << nsga_paths.size() << " optimized paths.");
    
    if (DebugConfig::getInstance().isEnabled("General.debug_mode_enabled")) {
        ResultSaver::saveResults(rrt_star_paths, nsga_paths, config);
    }

    DEBUG_LOG("TestRunner", "\nTest finished successfully.");

	return 0;
}